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Abstract 


The  extreme  Test  Bed  is  a  small-scale  remotely  controlled  truck  that  has  been 
retrofitted  with  a  microprocessor,  sensors,  servos  and  actuators  to  experiment  with 
different  concepts  and  software  applications  in  an  indoor  environment.  Two 
intelligence  levels  are  exploited.  A  low  level  vehicle  control  is  realized  by  the 
MPC555  microprocessor  for  reactive  avoidance,  wandering  state  and  A-to-B 
mobility.  The  second  is  a  navigation  control  level  presently  in  development.  This 
technical  memorandum  focuses  on  the  first  control  level.  It  details  the  MPC555 
functionalities,  program  functions,  mathematics,  algorithms  and  components  used  to 
control  the  extreme  Test  Bed.  The  objective  is  to  test  technologies  and  software 
capabilities  before  application  to  more  expensive  platforms.  A  second  goal  is  to 
become  familiar  with  a  real-time  operating  system  executing  several  tasks  in  parallel. 


Resume 


L'eXtreme  Test  Bed  est  un  petit  vehicule  tout-terrain  telecommande  qui  a  ete  modifie 
par  l'ajout  d'un  microprocesseur,  de  capteurs,  de  servos  et  d'actuateurs  pour 
experimenter  differents  concepts  et  algorithmes  dans  un  environnement  interieur. 
Deux  niveaux  d'intelligence  sont  explodes.  Le  controle  de  base  du  vehicule  est  realise 
a  l'aide  du  microprocesseur  MPC555  pour  executer  des  evitements  d'obstacles,  un  etat 
d'errance  ou  une  mobilite  d'un  point  A  a  un  point  B.  Le  second  niveau  est  un  module 
de  controle  de  la  navigation  du  robot  qui  est  presentement  en  developpement.  Ce 
memoire  technique  traite  du  premier  niveau  de  controle.  II  detaille  les  fonctionnalites 
du  MPC555,  les  fonctions  du  programme  informatique,  les  formules  mathematiques, 
les  algorithmes  de  programmation  et  les  composantes  du  systeme  employees  pour 
controler  1'eXtreme  Test  Bed.  L'objectif  est  de  tester  les  capacites  de  diverses 
technologies  et  programmes  informatiques  avant  de  les  appliquer  sur  des  plate-formes 
plus  dispendieuses.  Un  second  but  est  de  se  familiariser  avec  un  systeme  d'operation 
en  temps  reel  qui  execute  plusieurs  taches  en  parallele. 
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Executive  summary 

Background 

In  future  combat  fields,  unmanned  vehicles  will  play  a  vital  role.  Civilian  and  military 
communities  are  developing  robotic  systems  with  sufficient  autonomy  to  replace  and 
assist  humans  or  increase  their  capabilities  to  achieve  specific  tasks. 

The  Autonomous  Intelligent  Systems  project  (AIS-Land)  at  Defence  R&D  Canada  - 
Suffield,  works  on  conceptualization,  invention  and  application  of  technologies  to 
develop  innovative  unmanned  ground  vehicles. 

Results 

In  Fall  2003,  a  project  using  a  small-scale  prototype,  the  eXtreme  Test  Bed  (XTB), 
started.  The  goal  was  to  test  and  become  familiarized  with  technologies,  sensors,  a 
real-time  embedded  system,  a  microprocessor,  concepts  and  algorithms  before 
installing  them  on  bigger  and  more  expensive  platforms.  It  is  developed  for  indoor 
applications. 

This  document  focusses  on  the  low  level  control  of  the  robot,  i.e.  reactive  obstacle 
avoidance,  wandering  mode  and  rudimentary  A-to-B  mobility.  The  XTB  gets  data 
from  the  different  sensors  and  controls  the  motor  and  servos.  It  details  the  vehicle 
components  and  the  way  they  are  controlled.  The  platform  used  is  a  Tamiya  TXT-1 
Extreme  R/C  Monster  Truck  retrofitted  to  become  an  autonomous  test  bed.  The 
sensors  allow  understanding  and  interaction  with  the  environment.  Odometry  and 
range  detection  are  used  to  implement  the  robot  servo-control. 

An  important  part  of  the  document  is  the  program  algorithm.  In  the  implementation 
two  tools  are  used:  a  high-performance  microprocessor,  the  MPC555  from  Motorola, 
and  a  real-time  operating  system  called  RTEMS.  The  project  aims  to  become  familiar 
with  those  powerful  tools  so  they  can  be  applied  to  other  systems. 

Significance 

The  first  level  control  is  completed.  Teleoperation  and  simple  autonomous  modes  like 
reactive  obstacle  avoidance,  wandering  mode  and  simple  A-to-B  mobility  are 
available.  This  modular  first  control  allows  one  to  add  higher  control  levels  sending 
data  to  the  basic  level  that  controls  the  vehicle.  Thus,  it  is  possible  to  test  different 
navigational  and  arbitrational  algorithms  with  very  few  modifications  to  the  first  level 
control. 
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Future  Results 


By  adding  a  camera  to  the  infrared  range  detection,  the  system  will  get  a  better 
understanding  of  the  environment.  It  will  involve  the  implementation  of  a  second 
control  level  providing  navigation  commands  to  the  first  control  level  by  using  image 
data  to  solve  navigation  algorithms  and  then  verify  arbitration  rules.  This  more 
complex  intelligence  is  under  development. 

Another  eventual  work  is  multi-robot  applications.  The  aim  will  be  to  familiarize  with 
communication  systems  and  development  of  algorithms  to  realize  missions  by 
interaction  of  several  robots. 


Vincent,  I.  2004.  The  extreme  Test  Bed  vehicle  for  indoor  environments.  DRDC  Suffield 
TM  2004-262.  Defence  R&D  Canada  -  Suffield. 
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Sommaire 


Contexte 

Sur  les  champs  de  bataille  du  futur,  les  vehicules  non-pilotes  joueront  un  role  vital. 

Les  communautes  civiles  et  militaires  developpent  des  systemes  robotises  avec 
suffisamment  d'autonomie  pour  augmenter  les  capacites  des  humains,  les  remplacer 
ou  les  assister,  afin  d'executer  des  taches  specifiques. 

Le  projet  des  Systemes  autonomes  intelligents  (AIS-Land)  a  R  &  D  pour  la  Defense 
Canada  -  Suffield,  travaille  a  la  conceptualisation,  a  l'invention  et  a  l'application  de 
technologies  pour  developper  des  vehicules  terrestres  autonomes  innovateurs. 

Res  u  I  tats 

A  l'automne  2003,  un  projet  utilisant  un  prototype  de  petite  taille,  1'eXtreme  Test  Bed 
(XTB),  vit  le  jour.  Son  objectif  etait  de  tester  et  de  se  familiariser  avec  des 
technologies,  des  capteurs,  un  systeme  d'operation  en  temps  reel,  un  puissant 
microprocesseur,  ainsi  que  des  concepts  et  des  algorithmes  avant  de  les  installer  sur 
des  plate-formes  plus  grosses  et  plus  dispendieuses.  Le  vehicule  est  developpe  pour 
des  applications  d'interieur. 

Ce  document  traite  du  premier  niveau  de  controle  du  robot,  c'est-a-dire  l'evitement 
d'obstacles,  un  mode  d'errance  et  une  simple  mobilite  d'un  point  A  a  un  point  B.  II 
obtient  les  donnees  de  divers  capteurs  et  controle  le  moteur  et  les  servos.  Les 
composantes  du  vehicules  sont  detaillees  ainsi  que  la  fa£on  dont  elles  sont  controlees. 
La  plate-forme  utilisee  est  un  camion  Tamiya  TXT-1  Extreme  R/C  Monster  Truck 
modifie  pour  devenir  un  banc  d'essai  autonome.  Les  capteurs  permettent  de 
comprendre  et  d'interagir  avec  l'environnement.  L'odometrie  et  la  detection  de  la 
distance  des  objets  sont  utilises  pour  controler  le  robot. 

Une  partie  importante  du  document  est  l'algorithme  du  programme  informatique.  Pour 
executer  le  programme,  deux  outils  sont  utilises:  un  microprocesseur  haute 
performance,  le  MPC555  de  Motorola,  et  le  systeme  d'operation  en  temps  reel 
RTEMS.  Le  projet  vise  a  se  familiariser  avec  ces  deux  puissants  outils  pour  ensuite 
pouvoir  les  employer  dans  d'autres  systemes. 

Portee  des  resultats 

Le  premier  niveau  de  controle  est  complete.  La  teleoperation  et  de  simples  modes 
autonomes  sont  disponibles  tels  l'evitement  d'obstacle  par  simple  reaction,  un  mode 
d'errance  et  une  mobilite  rudimentaire  d'un  point  A  a  un  point  B.  Ce  premier  controle 
modulaire  permet  d'ajouter  des  niveaux  de  controle  plus  eleves  qui  envoient  des 
donnees  au  niveau  de  base  controlant  le  vehicule.  Ainsi,  il  est  possible  de  tester 
differents  algorithmes  de  navigation  et  d'arbitration  en  effectuant  peu  de 
modifications  au  premier  niveau  de  controle. 
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Travaux  futurs 


En  ajoutant  une  camera  au  systeme  en  plus  de  la  detection  infrarouge,  le  robot 
obtiendra  une  meilleure  comprehension  de  son  environnement.  Cela  impliquera 
l'application  d'un  second  niveau  de  controle  qui  foumira  les  commandes  de  navigation 
au  premier  niveau  de  controle  en  utilisant  les  donnees  des  images  de  la  camera  et  de 
la  detection  infrarouge  pour  resoudre  des  algorithmes  de  navigation  et  verifier  les 
regies  d'arbitration.  Cette  intelligence  plus  complexe  est  presentement  en 
developpement. 

Eventuellement,  des  applications  de  cooperation  multirobot  seront  etudiees.  Le  but 
sera  de  se  familiariser  avec  les  systemes  de  communication  et  le  developpement 
d'algorithmes  pour  realiser  des  missions  impliquant  l'interaction  de  plusieurs  robots. 


Vincent,  I.  2004.  The  extreme  Test  Bed  vehicle  for  indoor  environments.  DR  DC  Suffield 
TM  2004-262.  R  &  D  pour  la  defense  Canada  -  Suffield. 
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1.  Introduction 


The  Autonomous  Intelligent  Systems  project  (AIS-Land)  at  Defence  R&D  Canada  - 
Suffield,  works  on  conceptualization,  invention  and  application  of  technologies  to 
develop  innovative  unmanned  ground  vehicles.  The  extreme  Test  Bed  (XTB)  is  a  small 
scale  unmanned  ground  vehicle  used  for  indoor  applications  and,  eventually, 
multi-robot  research  activities.  The  prototype  being  inexpensive,  it  is  a  good 
experimental  platform  prior  to  applying  technologies  to  more  expensive  vehicles.  It  is 
suitable  to  experiment  with  different  concepts,  technologies,  interfaces  and  software 
algorithms. 


The  XTB  is  a  small  RC  truck.  Specifically,  it  is  a  Tamiya  TXT- 1  Extreme  R/C  Monster 
that  has  been  retrofitted  with  sensors,  a  MPC555  microprocessor,  two  scanning 
bumpers  and  an  active  platform.  The  first  control  level  is  realized  by  the  40MHz 
MPC555  hoard  which  controls  every  truck  component.  It  reads  the  encoder,  controls 
the  motor  speed,  the  steering  angle  and  the  bumper  servo  angle,  maintains  the  platform 
balance,  monitors  the  inclinometer  and  the  suspension  sensors,  and  performs  the 
infrared  ranging  for  obstacle  avoidance.  It  executes  teleoperation,  a  wandering  state 
and  rudimentary  A-to-B  mobility. 


In  future  work,  a  second  control  level  will  be  provided  by  a  533MHz  PC-104  board.  It 
will  run  Linux  and  probably  Carmen  control  software.  This  software  provides 
mapping,  localization,  path  planning  and  navigation.  The  PC  104  board  will  also  be  in 
charge  of  monitoring  camera  data.  Inter  Process  Communication  (IPC)  will  make  the 
connection  between  the  two  CPUs  to  send  the  navigation  commands  from  the  PC  104  to 
the  MPC555,  and  receive  data  acquisition  and  vehicle  states  transmitted  by  the 
MPC555  to  the  PC  104. 


The  ultimate  goal  of  the  project  is  to  produce  a  small  autonomous  vehicle  to  test  and 
become  familiarized  with  technologies,  concepts,  the  MPC555  and  the  real-time 
operating  system  RTEMS,  and  realizing  missions,  individually  or  in  multi -robot  tasks. 
In  parallel,  a  teleoperation  mode  is  under  development,  using  a  radio  frequency  remote 
control  to  manually  direct  the  prototype. 


This  technical  memorandum  focuses  on  the  MPC555  controller  for  the  creation  of  real 
world  behaviors.  The  next  section  presents  the  robot  description,  detailing  the 
components  and  the  sensors  with  their  specifications.  The  following  section  describes 
the  real-time  operating  system,  the  compiler  and  the  debugger.  Before  presenting  the 
project  algorithms  from  section  7,  section  6  details  different  functions  of  the  program 
and  the  control  of  each  component  of  the  vehicle.  Finally,  Annex  A  explains  the  servo’s 
calibration  and  Annex  B  lists  the  acronyms  used  in  the  document. 
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Figure  1:  The  extreme  Test  Bed  platform. 

2.  Vehicle  description 


This  section  details  the  components  of  the  vehicle  and  the  sensors  used  to  control  the 
robot  and  get  information  about  its  environment.  To  support  the  description,  a  picture 
of  the  prototype  is  presented  in  Figure  1 . 


2.1  Components 

The  XTB  is  a  Tamiya  TXT- 1  Extreme  R/C  Monster  Truck  retrofitted  to  become  an 
autonomous  test  bed.  The  light  aluminum  frame  supports  four  wheels  with  4.17”  x  6.5” 
tires.  The  truck  comes  with  a  Tamiya’s  Mabuchi  RS-540  high  torque  DC  motor 
providing  540  Watts  of  power.  The  dual-motor  gearbox  generates  a  34:1  gear  ratio. 

Two  drive  axles  transmit  the  power  to  the  wheels,  providing  a  four-wheel  drive  vehicle. 
The  steering  system  is  composed  of  independent  front  and  back  steering.  High  torque 
servos  actuate  the  dual  tie -rod  to  orient  the  wheels.  They  are  Hobbico  I4CAMol91 
Command  CS-70MG  Super  torque  2BB  servos,  generating  up  to  7.7  kg-cm  of  torque  at 
4.8  Volts. 

The  vehicle  requires  a  7.2  Volt  battery  to  operate.  The  Venom  racing  3000  NIMF1 
power  pack  delivers  3000  mah  to  the  system.  A  Novak  Super  Rooster  reversible  speed 
control  is  used  to  regulate  the  power  sent  to  the  motor  and  the  servos.  Moreover,  a 
DC/DC  converter  is  added.  It  is  a  Datel  single  output  UNS  series  non-isolated,  3.3  and 
5  Volt,  3  Amp  DC/DC  power  converter,  low  cost,  high  efficiency  (90-92%),  low  output 
noise  and  wide  range  inputs. 

The  cantilever  suspension  shown  in  Figure  2,  is  composed  of  oil  dampers  with  4  inch 
coils,  offering  smooth  and  adjustable  damping.  Four  servos  activate  the  cantilevers  to 
push  the  arms  compressing  the  shocks.  The  suspension  servos  are  Hitec  HS-5645MG 
digital  ultra  torque  servos.  The  speed  operation  is  0.23sec/60deg  at  4.8V  with  an  output 
torque  of  10.3  kg-cm  at  4.8V. 
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Figure  2:  Cantilever  suspension  activated  by  a  high  torque  servo  rotating  the  piston  screw. 


The  servos  send  an  electrical  input  determining  the  position  of  the  motor  armature.  In 
fact,  the  duty  cycle  of  a  periodic  rectangular  pulse  train  determines  this  position.  The 
signal  period  is  22.2  ms.  The  pulse  length  varies  from  1.0  to  2.0  ms  with  the  pulse 
being  1.5  ms  when  the  arm  is  centered.  For  the  active  suspension,  the  position  of  the 
servo  arm  is  less  useful  than  the  rotation  speed.  To  create  this  effect,  the  servo  has  been 
modified.  The  feedback  sensor  is  removed  and  replaced  by  an  equivalent  circuit, 
making  the  servo  rotate  continuously  in  the  appropriate  direction  as  long  as  the  signal 
remains.  This  means  that  a  constant  pulse  length  commands  the  servo  to  go  to  a  certain 
position.  Being  modified,  it  never  reaches  this  stable  position  and  rotates  at  a  constant 
speed.  Thus,  the  servo  screws  or  unscrews  the  piston  pushing  the  suspension  cantilever. 

The  platform  is  tele-operated  with  a  Fatuba  Conquest  FP-T5NLP  5  channels  FM  pulse 
code  modulation  radio  control.  The  corresponding  receiver  is  a  Fatuba  FP-R1051P 
PCM  72.990  MHz  FM  5  channels  micro  receiver. 

Finally,  the  main  processor  of  the  robot  is  a  PowerPC  based  SS555  from  Intec 
Automation  Inc.  The  40  MHz  MPC555  Motorola  embedded  microprocessor  is  a  RISC 
PowerPC  core  with  a  floating-point  unit.  To  realize  the  XTB  project,  it  has  8  pulse 
width  modulation  channels,  32  10-bit  analog-to-digital  converter  pins,  2  time  processor 
units  with  16  independent  channels  each  and  2  RS-232  serial  communication  interface 
ports.  It  supports  the  Macraigor  Wiggler  and  P&E  ICDPPC  In-Circuit  Emulator/Debug 
cable. 

2.2  Sensors 

The  robot  needs  sensors  to  sense  its  environment  and  adapt  its  behavior.  Twelve 
infrared  object  detectors  are  installed,  six  on  the  front  bumper  and  six  at  the  back.  They 
are  equally  distributed  every  7  cm.  Figure  3  shows  a  bumper  with  the  selected  sensors: 
the  SHARP  IR  GP2D12  sensors  manufactured  by  Acroname.  These  detectors  are 
small,  consume  very  little  current  and  offer  good  ambient  lighting  immunity.  They  use 
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Figure  3:  SHARP  GP2D12  Infrared  sensors  for  range  detection. 


a  triangulation  method  and  a  small  CCD  array  to  compute  the  object  distance.  The 
non-linear  output  analog  signal  varies  between  0  to  3  Volt  and  is  updated 
approximately  every  32  ms.  The  detectors  fire  continuously  and  require  25  mA  of 
continuous  current.  An  analog-to-digital  converter  converts  the  distance  measurements 
in  numeric  values.  The  range  detection  is  10  to  80  cm,  however,  with  a  non-re flective 
object,  this  length  is  reduced.  Experimentally,  the  effective  range  has  been  evaluated  to 
be  10  to  25  cm.  Over  this  boundary,  the  readings  are  not  sufficiently  accurate  to  be 
useful.  Due  to  the  bell  shape  of  the  output  sensor  signal,  below  10  cm  the  voltage 
appears  similar  to  ranges  over  10  cm.  To  avoid  misinterpretation  of  the  output,  the 
range  used  doesn’t  cover  the  0-10  cm  detection  range. 

The  top  platform  of  the  truck  can  be  pitched  or  rolled  by  controlling  the  shock 
damping.  By  compressing  a  suspension,  the  platform  tilts.  Thus,  it  is  possible  to 
program  an  active  suspension  to  keep  the  platform  balanced.  To  determine  the  tilt 
angle,  a  tilt  sensor  is  positioned  in  the  middle  of  the  platform.  A  dual  axis  analog  tilt 
sensor  from  Crossbow,  model  CXTA02,  offers  ±75  degree  range  with  ±20  degree 
linear.  It  is  fully  conditioned,  its  resolution  is  0.05  degree  and  its  sensitivity  is  35  ±2 
mV/degree.  Moreover,  a  linear  resistor  sensor  (35K)  is  installed  on  each  piston  to 
measure  its  current  position.  It  allows  to  do  not  force  the  servos  when  trying  to 
overpass  the  piston  limits,  and,  for  the  platform  balance  controller,  to  keep  the  piston  as 
much  as  possible  in  its  center.  This  prevents  the  four  suspensions  from  reaching  their 
maximum  or  minimum  limit. 

To  measure  the  vehicle  speed,  an  optical  encoder  is  mounted  on  the  engine  shaft.  The 
Servo-Tek  PM  series  face-mount  encoder  is  a  small  size  and  low  cost  solution.  It  gives 
a  single  channel  square  wave  output  used  for  motor  speed  control  and  speed  indication. 
The  input  voltage  is  5V  and  current  16  mA.  It  produces  60  pulses  per  revolution  for  a 
maximum  shaft  speed  of  12  000  rpm  and  accepts  rotation  in  either  direction. 

To  get  odometry  data,  a  gyroscope  is  added  to  the  system.  The  MicroStrain  3DM-G 
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Gyro  Enhanced  Orientation  Sensor  provides  orientation  and  angular  velocity.  It 
incorporates  3  triaxial  magnetometers,  3  angular  rate  gyros  and  3  orthogonal  DC 
accelerometers  for  the  three  axes.  A  12  bit  analog-to-digital  converter  converts  the 
output  signals.  An  embedded  microcontroller  then  provides  the  orientation  angles 
(pitch,  roll  and  yaw)  by  RS-232  serial  communication  at  38.4  kbaud  to  the  MPC555. 
For  all  axes,  the  orientation  range  is  360  degrees,  the  angular  velocity  range  is  ±300 
degrees/second,  the  accelerometer  range  is  ±2  G’s  and  the  magnetometer  is  ±1  Gauss. 
The  orientation  angle  resolution  is  less  than  0.1  degrees.  The  output  data  rate  is  100  Hz. 

3.  Software  supports 


This  section  defines  the  compiler,  the  debugger  and  the  real-time  operating  system  used 
in  the  XTB  project. 


3.1  Compiler 

The  GCC  is  the  high  quality  GNU  C  compiler.  It  supports  C  and  C++  language 
programs.  Finally,  it  is  frequently  used  on  machines  running  Finux. 


3.2  Debugger 

The  powerful  open-source  GNU  program  debugger  called  GDB  is  utilized  for  the 
project. 


3.3  Real-time  Operating  System 

RTEMS  stands  for  Real  Time  Executive  for  Multiprocessor  Systems.  It  is  a  free 
real-time  system  and  it  is  supported  by  the  MPC555  target.  It  is  based  upon  the  free 
GNU  tools  and  the  open  source  C  Eibrary  Newlib.  The  real-time  executive  provides  a 
high  performance  environment  for  embedded  systems. 

RTEMS  has  an  object-oriented  nature,  encouraging  modular  applications.  The 
real-time  operating  system  is  appropriate  for  applications  with  rigorous  requirements, 
not  just  the  results  of  computations,  but  also  critical  time  constraints  to  produce  the 
results.  Finally,  it  has  multitasking  capabilities,  so  it  can  manage  several  concurrent 
processes. 

Table  1  lists  the  different  managers  featuring  in  the  RTEMS  kernel.  More  information 
about  RTEMS  and  details  about  its  functions  and  managers  can  be  found  in  [1]. 
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Categories 

Managers 

Time 

Rate  monotonic 

Clock 

Timer 

Communication  and  synchronization 

Semaphore 

Message  queue 

Event 

Signal 

Memory 

Partition 

Region 

Dual  ported  memory 

Others 

Initialization 

Task 

Interrupt 

Input/output 

Fatal  error 

User  extension 

Multiprocessing 

Table  1:  RTEMS  managers 


4.  Program  Functionnalities 


This  section  describes  the  control  of  each  component  of  the  vehicle.  There  arc  also 
explanations  about  the  arguments  chosen  for  the  Motorola  functions  and  the 
mathematical  equations  employed. 


4.1  Pulse  Width  Modulation  (PWM) 

The  signal  period  required  by  the  motor  and  the  servos  is  22.2  milliseconds.  These 
components  arc  controlled  by  PWM  command  or  by  modulation  of  the  pulse  width. 
There  arc  eigth  PWM  channels  on  the  MPC555  and  they  command  the  motor  speed, 
the  back  bumper  angles,  three  suspension  actuators  and  the  front  steering  angle.  Due  to 
the  lack  of  channels  available,  the  front  bumper  and  the  front-right  suspension  actuator 
servos  arc  controlled  by  two  Time  Processor  Unit  (TPU)  PWM  channels.  The  PWM 
pulses  sent  to  the  components  arc  obtained  by  multiplying  the  duty  cycle  by  the  PWM 
signal  period  of  22.2  milliseconds.  The  duty  cycle  is  evaluated  by  the  time  processor 
unit  channels  receiving  signals  from  the  remote  control.  The  duty  cycle,  proportion  of 
the  signal  that  is  high  during  one  period,  is  constrainted  between  4.2%  and  9.5%.  This 
is  the  limits  for  the  servos.  The  systems  have  been  calibrated  to  get  a  relation  between 
the  PWM  command  and  the  resulting  speed  or  angle.  The  calibration  graphs  arc 
presented  in  Annex  A.  Variation  of  the  PWM  command  varies  motor  or  servo  speed  or 
angle.  The  infrared  sensor  bumpers  and  the  steerings  use  the  PWM  command  for 
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position  control.  The  motor  and  the  suspension  activators  use  it  for  speed  control. 


4.2  Time  Processor  Unit  3  (TPU3) 

The  MPC555  comprises  an  enhanced  version  of  the  original  TPU  designed  for  timing 
control.  The  TPU3  operates  simultaneously  with  the  Central  Processor  Unit  (CPU), 
minimizing  the  CPU  interrupt  services.  It  consists  of  two  time  bases,  each  having  16 
independent  timer  channels.  The  general  functions  of  the  TPU  arc  defined  in  [2]  and  a 
specific  reference  about  the  TPU3  is  [3,  ch.  17].  To  get  more  information  about  the 
TPU,  consult  [4], 

4.2.1  TPU3  Setup 

The  TPU3  setup  needs  to  set  four  registers  [3,  ch.  17],  The  TPU  configuration 
register  module  TPUMCR  =  0x0020  means  the  TPU3  clocks  are  enabled,  no 
prescaler  is  used  for  the  time  count  registers  (TCR1  and  TCR2),  the  TPU3 
doesn’t  operate  in  emulation  mode,  the  assignable  registers  are  accessible 
from  user  and  supervisor  privilege  level  and  the  TPU3  mode  is  selected.  The 
time  count  register  used  is  the  TCR1.  The  configuration  register  module  3 
TPUMCR3  =  0x005f  means  that  an  enhanced  64  prescaler  divides  the 
Intermodule  Bus  (IMB)  clock.  The  prescaler  was  determined  to  detect  pulses 
well.  Too  low  frequency  would  result  in  undetected  edges  and  too  high 
frequency  would  involve  a  measurement  window  too  small  to  cover  the  entire 
signal  period.  The  TPU3  interrupt  configuration  register  TICR  requires  setting 
the  channel  interrupt  request  level  field  and  the  interrupt  level  byte  select  field. 
A  priority  level  of  five  is  selected  using  the  IRQ[0:7]  byte. 

4.2.2  New  Input  Transition  /  Input  Capture  Function  (TPU_NITC) 

This  function  described  in  [5]  can  detect  rising  and/or  falling  input  transitions. 
It  records  the  TCR  value,  getting  the  number  of  ticks  occurring  between  two 
edges.  It  is  possible  to  determine  the  duty  cycle  by  counting  the  ticks  between 
two  consecutive  rising  and  falling  edges.  This  function  is  used  to  measure  the 
duty  cycle  of  the  signal  sent  by  remote  control  to  the  receiver.  The  next 
example  shows  the  TPU  and  the  channel  used,  an  high  interrupt  priority  and  a 
continuous  mode  on  rising  and  falling  edges  arc  chosen,  the  TCR1  counts  the 
clock  ticks,  no  channel  link  is  utilized,  and  an  interrupt  is  asseted  every  two 
edges. 

Example:  tpu_nitc_init_tcr_mode(tpua,  channel_speed, 
TPU_PRIORITY_HIGH,  TPU_NIT C_RIS IN G_FALLIN G ,  2, 

TPU_NITC_C  ONTINUOUS  ,TPU_NITC_TCR  1 ,  TPU_NITC_NOLINK, 
TPU_NITC_START_LINK_CHANNEL_0,  TPU_NITC_LINK_ONE, 
TPU_NITC_INTERRUPT). 
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This  function  gets  the  duty  cycle  for  the  servos,  the  motor  and  the 
manual/automatic  selector.  The  40MHz  processor  frequency  is  divided  by  a 
64  prescaler,  thus  a  TCR  tick  occurs  every  1.6  microseconds.  The  TPU  counts 
these  TCR  ticks.  To  get  the  duty  cycle,  multiply  the  tick  count  by  the  TCR 
sample  time  and  divide  by  the  signal  period. 


_  TC(Ptjck) 


Where 

DC  =  duty  cycle 

TC  =  tick  count 

P  =  signal  period  (22.2  ms) 

Ptick=  TCR  tick  period  (1.6  us). 

One  problem  encountered  is  that  the  function  TPU_NITC  doesn’t  allow  the 
user  to  select  to  start  on  a  rising  or  a  falling  edge  when  the  mode  rising  and 
falling  edges  is  selected.  The  reading  can  be  the  tick  count  between  the  rising 
and  the  falling  edges  of  the  period  or  between  the  falling  and  the  rising  edges 
of  the  period.  One  is  the  complement  of  the  other.  In  term  of  duty  cycle,  the 
complement  is  100%  minus  the  result.  Another  problem  is  that  when  using 
several  tpu  channels,  strange  duty  cycles  arc  sometimes  obtained  like  420%. 
To  solve  the  problem,  the  defective  tpu  channel  is  reset  to  get  a  good  duty 
cycle.  Also,  to  avoid  overpassing  the  limitations  of  the  motor  and  the  servos, 
when  the  duty  cycle  is  over  9.5%,  the  previous  PWM  command  is  not 
modified. 

The  NITC  function  is  used  to  measure  the  duty  cycle  of  the  remote  control 
signals. 

4.2.3  Frequency  Measurement  Function  (TPU_FQM) 

This  function  described  in  [6,  7]  counts  completed  pulses  on  a  TPU  channel 
within  a  specified  time  accumulation  window.  The  options  arc  single  time 
window  or  continuous  mode,  and  rising  or  falling  edge  capture.  An  interrupt 
occurs  when  the  time  window  is  complete.  For  the  present  case,  the  next 
example  shows  which  TPU  and  channel  arc  used,  a  continuous  mode  is 
chosen  to  measure  frequency  of  rising  edges,  the  interrupt  priority  is  high  and 
the  window  size  is  8000  (maximum  window  size). 

Example:  tpu_fqm_init(tpua,  channel_enc,  TPU_PRIORITY_FIIGH, 

TPU_F QM_RIS  E_EDGE_C  ONT,  TPU_F QM_RIS  E, 

TPU_F  QM_TCR  1 ,0x8000). 
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This  function  counts  the  encoder  pulses  and  determines  the  wheel  speed. 
Reading  the  encoder  output  means  counting  the  number  of  ticks  during  the 
sample  time. 


W(64)(4) 

LS  —  f 

Jsys 


Where 

ts  =  sample  time  [s] 

W  =  window  size  (8000) 

fays  =  microprocessor  frequency  (40  MHz). 

The  64  prescaler  is  set  in  the  tpu  setup  function.  The  puipose  and  origin  of  the 
constant  4  is  unknown.  This  prescaler  was  established  by  comparing  the  TPU 
reading  to  the  expected  reading  using  a  64  prescaler.  To  get  the  frequency, 
divide  the  tick  count  by  the  sample  time.  This  is  the  engine  shaft  frequency. 

To  get  the  wheel  frequency,  it  is  divided  by  the  gear  ratio  (34:1).  Then  the 
wheel  speed  is  expressed  in  rpm. 


t  —  TC 
Jw  ~  ts(GR ) 


rpm  = 


fA  60) 

fsys 


Where 

TC  =  tick  count 

ts  =  sample  time  [s] 

fw  =  wheel  frequency  [Hz] 

GR  =  gear  ratio  (34/1) 

lpm  =  wheel  speed  [rpm] 

fsys  =  microprocessor  frequency  (40  MHz). 

The  FQM  function  gives  an  accurate  measure  of  the  wheel  speed  for  this 
project.  Due  to  the  limited  time  window  size,  this  function  wouldn’t  be 
appropriate  for  a  low  speed  system. 
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4.3 


Remote  Control 


Each  TPU  channel  is  configured  to  measure  rising  and  falling  edges  in  continous  mode. 
Thus,  it  is  possible  to  know  the  duty  cycle  for  each  signal  sent  by  the  remote  control 
and  received  by  the  receiver.  There  arc  three  signals  controlled  remotely:  wheel  speed, 
steering  angle,  and  the  manual/automatic  selector.  Two  additionnal  channels  arc 
available.  The  manual  control  allows  the  user  to  position  the  robot  and  perform  tests 
manually.  The  automatic  mode  is  used  for  autonomous  control. 

4.4  Serial  Communication  Interface  (SCI) 

The  commands  arc  received  and  the  data  transmitted  by  the  serial  communication 
interface.  The  MPC555  controls  two  RS-232  ports.  The  first  serial  port  receives 
commands  to  control  the  vehicle  in  automatic  mode  and  to  send  data  about  sensors  and 
the  state  of  each  component.  The  second  serial  port  communicates  with  the  gyroscope 
to  get  orientation  and  acceleration  data,  both  useful  to  calculate  odometry. 

The  serial  port  parameters  arc  a  9600  baud  rate  for  SCI1,  and  38400  for  SCI2.  The 
gyroscope  needs  fast  communication. 

4.5  Infrared  range  detectors  (IR) 

To  detects  obstacles,  the  XTB  has  been  equipped  with  two  active  bumpers,  each  having 
six  IR  sensors  equally  distanced  of  7  cm.  Each  sensor  output  is  converted  by  the 
analog-to-digital  converter  (ADC)  in  a  binary  output  .  The  adc_init  function  is 
configured  to  scan  the  16  channels  of  port  B  once.  The  next  formula  is  the  conversion 
in  Volt  of  the  10-bit  data. 


IR 


volt 


IRadc(V) 

R 


Where 

IRadc  =  ADC  binary  output 
IRvoit  =  ADC  output  converted  [V] 

V  =  power  supplied  to  the  ADC  (5V) 

R  =  10-bit  resolution  (1024). 

The  sensor  used  a  triangulation  process  to  determine  the  distance.  The  triangle 
hypotenuse  lentgh  is  calculated  as: 

IRhyp-ien  =  0.9195 IR6volt  +  1.6166 IR5volt  -  40.426/<*  +  125.12 IR^ 

- 13 1. 16IR;oIt  +  2.45941 Rvoh  +  69.999 
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Where 


IRvoit  =  ADC  output  converted  [V] 

IRhyp-ien  =  hypotenuse  length  [cm]. 

The  lineal-  distance  of  an  object  is  finally  determined  by  finding  the  cosine  of  the 
bumper  angle. 


IR/in-dist  —  f  R  hyp —ten  COS  (  0  j 


Where 

IRiin-dist  =  the  lineal'  distance  to  the  object  [cm] 

IRhyp-ien  =  hypotenuse  length  [cm] 

0  =  bumper  angle  [rad]. 

The  resulting  distance,  expressed  in  centimeters,  is  the  projected  distance  on  the 
horizontal.  A  safety  process  verities  the  proximity  of  the  object.  If  the  measured 
distance  is  shorter  than  25  cm  and  the  obstacle  is  in  the  same  direction  as  the  vehicle 
movement,  the  vehicle  is  immobilized.  The  detection  accuracy  depends  on  the  distance, 
the  reflectivity  of  the  objects  and  the  sensor  properties.  Consequently,  it  is  better  to  not 
use  this  data  to  model  the  environment,  but  to  establish  a  range  of  acceptable  object 
proximity.  Below  10  cm,  the  sensor  output  increases,  falsely  reporting  an  increase  in 
distance.  Thus,  if  an  object  comes  too  close  to  the  robot,  it  may  be  falsely  reported  as 
far  away.  Figure  4  shows  the  detection  range. 

1R  sensor  bumber 

m  “  “  “  “  m  0  cm 


10  cm 


Ml 

d 


25  cm 


Figure  4:  IR  bumper  detection  range  sketch 

4.6  Obstacle  avoidance 

It  is  verified  that  the  IR  sensor  measurements  are  not  below  25  cm.  This  range  was 
established  to  give  the  vehicle  enough  time  to  brake.  The  algorithm  notes  if  the 
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obstacle  is  at  the  front,  at  the  back,  both,  or  none.  The  sensor  that  detects  the  object  is 
not  important  because  the  curvature  radius  of  the  XTB  is  too  large  to  avoid  an  obstacle 
just  by  turning  the  steering  and  the  vehicle  is  also  too  large  to  do  a  quick  manoeuver. 
When  an  obstacle  is  seen  in  front,  the  best  strategy  is  to  reverse  and  to  try  another  path. 
Two  options  arc  possible.  If  the  obstacle  is  on  the  vehicle  path,  the  XTB  stops. 
Otherwise,  the  presence  of  the  obstacle  doesn’t  influence  the  robot  trajectory  and  travel 
can  continue.  In  teleoperation  mode,  the  vehicle  doesn’t  move  in  the  direction  of  a 
detected  obstacle.  In  wander  mode,  the  vehicle  stops,  then  turns  while  driving 
backward  from  the  obstacle  for  6  seconds,  then  goes  straight  forward.  It’s  a  simple 
avoidance  algorithm.  No  obstacle  detection  operates  while  the  vehicle  moves  backward 
from  the  obstacle.  If  a  new  one  is  encountered,  it  won’t  stop  the  movement  until  the 
end  of  the  avoidance  process.  This  could  be  improved.  However,  it  would  complicate 
the  algorithm  and  would  not  be  necessary  since  the  dead  period  is  short  and,  with  the 
vehicle  size,  the  environment  should  not  be  too  cluttered.  In  A-to-B  mobility  mode,  the 
obstacle  avoidance  option  is  not  integrated  yet.  However,  to  avoid  collision  the  vehicle 
stops  if  any  obstacle  is  detected  in  the  driving  direction. 


4.7  Speed  PID  Loop 

A  PID  servo-control  loop  controls  smoothly  the  vehicle  speed  given  a  speed  reference 
command.  The  system  was  experimentally  calibrated  through  trial  and  error,  to 
determine  the  PID  proportional,  derivative  and  integrative  constants  Kp ,  Kp>  and  Kj. 
Here  arc  the  steps  to  realize  the  servo-control  loop.  First,  get  the  error  for  the  nth 
sample  by  comparing  the  desired  speed  to  the  current  speed. 


e[n\  =  r pm ref  -  rpmcur 

Where 

e[n]  =  error  for  the  nth  sample 
r pm ref  =  reference  speed  [rpm] 
r  pm  cur  =  current  speed  [rpm]. 

Then  evaluate  the  correction  to  apply  with  the  next  formulas. 

corr  =  Ae[n]  +  Be[n  —  1]  +  C  J2ein\ 

A  =  Kp  +  0.5Kjts  +  fa 

Is 

B  =  0.5 Krts  -  fa 

ls 
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C  =  K, 


J2ein\  =  J2ein  ~  1]  +0-5  {e[n]+e[n  —  1])^ 


Where 

corr  =  correction  to  apply  to  the  motor  command 

J2e[n\  =  error  sum  at  the  nth  sample 

ts  =  TPU  sampling  time  to  get  the  encoder  reading  [s] 

Kp.  Kf).  K i  =  PID  constants  determined  by  trial  and  error 
(KP=  10,  Kj)  =  0.1,  K[  =  20). 

Finally,  the  correction  is  applied  to  the  motor  PWM  command  (Q)  by  addition  to  the  no 
movement  PWM  value  (PWMno-move). 

Q  —  PW Mno—move  -p  cot  t 

It  has  been  noticed  that  the  progression  is  smoother  forward  than  backward  due  to  a 
smaller  PWM  value  range  for  backward  motion  than  forward  speed. 

4.8  Platform  tilt  and  suspension  activation 

The  platform  of  the  robot  is  mounted  on  four  independant  cantilever  suspensions  that 
can  be  controlled  to  tilt  the  platform.  A  dual-tilt  sensor  indicates  the  inclination  of  the 
platform  and  a  linear  sensor  is  installed  on  each  suspension  piston  (actuator)  to  measure 
its  elongation.  Figure  5  shows  the  relation  between  the  components  and  sensors 
involved  in  the  servo-control  of  the  platform  balance. 

The  calculation  of  the  tilt  sensor  angles  is  presented  in  subsection  4.8.1,  the  two 
controllers  balancing  the  platform  arc  then  explained.  The  first  algorithm  developed 
(subsection  4.8.2)  uses  a  PID  loop  to  vary  the  servo  PWM  command  with  regards  to  the 
tilt  sensor  feedback.  The  second  controller  was  developed  to  improve  the  first  one  by 
centering  the  piston  position  and  respecting  the  piston  limits.  It  is  described  in  4.8.3. 

4.8.1  Tilt  sensor 

The  tilt  sensor  voltages  arc  converted  in  angle  and  then  compared  to  the 
references  ( 0re/_^,0re/_v)  =  (0,0)  to  keep  the  platform  horizontal.  A  PID  loop 
executes  this  comparison  and  modifies  the  PWM  command  sent  to  the 
suspension  actuators  to  control  the  suspension  height. 

First,  the  sensor  sensitivities  S  is  converted  in  Volt  per  radian. 
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References 


Rotation 


Figure  5:  The  extreme  Test  Bed  platform  balance  controler  diagram 
Si[V /red]  =  mSi[V/deg\ 

Where 

S  =  sensitivity 
i  =  x  or  y  axis. 

The  adc_init  function  is  configured  to  scan  the  16  channels  of  port  B  once. 
The  adc_get  function  reads  the  data.  The  10-bit  values  are  then  converted  to 
Volt  for  both  axes. 

T /  _  Vout-  lihovv) 

v  out— volt— i  —  r 

Where 

Vnut-i  =  ADC  binary  output 
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Platform 


Vout-volt-i  =  ADC  output  converted  [V] 

Pow  =  power  supplied  to  the  ADC  (5V) 

R  =  10-bit  resolution  (1024) 
i  =  x  or  y  axis. 

The  tilt  angles  <PA  and  <J>V  are  calculated  by  doing  the  difference  between  the 
output  and  the  reference  angle  (0,0)  voltage  Vref  for  the  x  and  y  axes,  and  then 
by  making  the  arcsine  of  the  comparison  with  the  sensitivity.  The  result  is 
then  converted  to  degrees. 

<P,  =  Ijj°  arcsin 

Where 

Vout-voit-i  =  ADC  output  converted  [V] 

<P,  =  tilt  angle  [deg] 

Vref-i  =  reference  voltage  for  a  zero  tilt  angle  [V] 

Si  =  sensitivity  [V/deg] 
i  =  x  or  y  axis. 

For  the  actual  tilt  sensor,  the  sensitivities  arc  Sx  =  0.0348 1 3  V  /deg  and 
Sy  =  0.033934V  / deg,  and  the  reference  voltages  for  a  zero  tilt  angle  arc 
Vref-X  =  2.525V  and  V„f-y  =  2.503V. 

4.8.2  Platform  PID  loop 

The  servo-control  of  the  platform  stability  is  done  with  a  PID  loop  controlling 
four  suspension  actuators  and  two  tilt  axis  measurements. 

First,  get  the  error  for  the  n,h  sample  time  error[n]  in  x  and  y. 

e\n]x  —  *&ref— x  *&cur—x 

=  *^Ve/— y  *&cur—v 


Where 
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e[n]  =  error  for  the  nth  sample 
(Pief  =  reference  tilt  angle  [deg] 

<Prur  =  current  tilt  angle  [deg]. 

Then,  evaluate  the  coiTections  to  apply  with  the  next  formulas. 

COrrx  =  Axe[n]x  +  Bxe[n  -  l]x  +  Cx  J2ein}x 
corry  =  Aye[n]y  +  Bye[n  -  l]y  +  Cy  ^2e[n]y 
Ax  =  KP_x  +  ().5K,_xts  +  ^ 

ls 

Ay  =  Kp_y  +  0.5K1_yts  +  ^f- 
Bx  =  0.5 K/_xts  -  ^ 

ls 

By  =  0.5 Kj_yts  -  ^ 

Cx  =  Kj_x 

Cy  =  Kl_y 

Ha-  =  J2e[n-Mx  +  0.5(e[n]x  +  e[n-l]x)ts 
Yje\r\y  =  He\n~  l]y  +  0.5(e[n\y  +  e[n-  1  ]y)t„ 


Where 

corr  =  correction  to  apply  to  the  motor  command 

<?[/?]  =  error  sum  at  the  nth  sample 

ts  =  TPU  sampling  time  to  get  the  encoder  reading  [s] 

Kp,  K[),  Ki  =  PID  constants  determined  through  trial  and  error 
(KP  =  200,  Kd  =  0.1,  Kj  =  10). 
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Finally,  the  corrections  arc  applied  to  the  commands  (Q)  by  addition  to  the 
zero  movement  PWM  value  ( PWMzero ).  Because  there  arc  two  corrections 
applied  on  each  actuator,  x  and  y  corrections,  and  one  can  be  in  the  opposite 
direction  than  the  other,  the  axes  orientations  have  to  be  carefully  studied.  The 
suspension  servos  arc  identified  by  their  position:  back-right  (BR),  back-left 
(BL),  front-right  (FR)  and  front-left  (FL). 


Qbr  =  PWMzero_BR  +  corrx  +  corr. 


Qbl  =  PWMzero_BL  +  corrx  -  corr. 


Qfr  =  PWMzero-FR  -  corrx  +  corr \ 


Qfl  =  PWMzero_FL  -  corrx  -  corry 


For  instance,  when  the  vehicle  tilts  on  the  right  side,  a  correction  in  x  is 
required.  The  BR  actuator  has  to  increase  the  suspension  height.  Similarly  for 
the  FR  actuator.  But  the  two  left  ones  have  to  decrease  the  suspension  height. 
The  commands  arc  finally  compared  to  the  PWM  limits  allowed  by  the  servo 
to  operate  properly. 

The  platform  movement  works  well,  but  is  slow.  A  possible  solution  would  be 
to  increase  the  screw  pitch,  which  will  require  enlarging  the  screw  diameter. 
Another  way  is  to  modify  the  piston  mount  on  the  cantilever  arm.  The  length 
is  reduced  to  get  a  bigger  influence  on  the  compression  for  a  smaller  piston 
deplacement.  This  modification  has  been  done  successfully. 

For  small  inclinations,  the  PID  loop  is  sufficient  to  keep  the  balance,  but  for 
important  ones,  few  seconds  arc  required  to  bring  the  platform  horizontal. 
Moreover,  the  continuous  loop  drains  lots  of  power  being  always 
compensating  for  tilt  difference.  A  tolerance  zone  should  be  added  to  avoid 
this  continuous  instability  around  the  reference  angles.  This  instability  is 
related  to  the  tilt  sensor  accuracy  and  the  ADC  resolution.  For  a  constant 
inclination,  the  ADC  reads  a  value  varying  around  the  correct  value. 

For  each  suspension,  a  sensor  reads  the  position  of  the  piston.  This  tool  is 
integrated  to  the  PID  loop  to  avoid  overshooting  the  physical  limits  of  the 
piston  and,  consequently,  damaging  the  servos.  If  the  maximum  or  minimum 
position  is  reached,  the  servo  stops  rotating. 
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4.8.3  Platform  balance  controller 


Another  control  algorithm  has  been  developed  to  avoid  draining  the  battery 
power.  It  also  maintains  the  platform  as  centered  as  possible  to  avoid  reaching 
the  ends  of  the  suspension  movement. 

This  algorithm  needs  the  tilt  angle  in  both  axes  and  the  position  of  each 
piston.  The  ADC  scans  64  times  each  channel  on  port  A  to  get  an  average 
value  for  each.  Then,  the  height  variation  required  for  each  suspension  is 
determined  with  the  next  formulas.  As  the  tilt  angles  arc  very  small,  the  small 
angle  approximation  is  used  (sin0  =  9). 


A/?fp  — 


(/Oj-WOy) 


FR  ~  - — 


A  hFL  = 


Ah.BR  ~ 


A hBL  =  td^+^A 


where 

A  hi  =  heigth  variation  for  the  ith  suspension 
1  =  vehicle  length  [m] 
w  =  vehicle  width  [nr] 
ch, .  <f\  =  tilt  angles  [deg]. 

To  center  the  piston,  an  average  value  p  is  evaluated. 


. . _  AhBL+AhgR+AliFL+AhFR 

P  ~  4 

The  way  the  height  variations  arc  calculated,  the  average  value  is  always  zero. 
Thus,  it  is  centered.  Then,  by  comparing  A  hi  to  p,  a  new  height  variation  A  ir¬ 
is  obtained. 


Ah  •  =  Ahj  —  p 
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To  convert  the  piston  height  variation  in  displacement,  a  factor  K  multiplies 
A h\.  This  factor  has  been  determined  through  trial  and  error  and  represents 
the  speed  of  convergence  of  the  servo.  The  piston  displacement  is  then 
evaluated  by  adding  the  result  to  the  piston  middle  position  mpi  and  by 
substracting  the  current  position  Zi- 


Azi  =  K  A  h\  +  mpi  -  Zi 


Where 

mpi  =  middle  position  of  piston  i 
Zi  =  piston  current  position 

K  =  convergence  factor  (set  to  for  the  experiment  K  =  5) 

A  hi  =  height  variation  for  the  ith  suspension 
A  Zi  =  piston  displacement. 

The  relation  between  the  piston  speed  and  the  servo  PWM  value  is  displayed 
graphically  in  the  Annex  A.  As  can  be  seen,  the  relation  isn’t  linear.  It  seems 
to  be  more  of  the  4th  order.  However,  a  linear  approximation  is  sufficient  for 
the  present  application.  Knowing  the  sampling  time  and  the  desired 
displacement,  the  speed  is  thus  known.  Here  is  the  pulse  width  formula: 


PWMi=Ai^+Bi 


where 

PWMj  =  pulse  width  modulation  of  piston  i 
A t  =  sampling  time  [s] 

A,  B  =  lineal-  constants. 

To  avoid  instability  of  the  system  around  the  tilt  reference  angles,  a  tolerance 
area  is  delimited.  The  algorithm  considers  a  tilt  of  <PX  ±  0.8°  and  T>y  ±  0.8° 
sufficient.  In  this  zone,  the  pulse  width  is  set  to  stop  the  piston. 

Some  safety  processes  are  added  to  the  algorithm.  First,  the  piston  position  is 
limited  to  not  break  the  linear  sensor  and  force  the  servo.  When  a  piston  end 
is  reached,  the  PWM  value  is  set  to  stop  the  servo  in  the  end  direction. 
Second,  the  pulse  width  is  also  limited  to  avoid  excessive  servo  speed. 
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4.9  Odometry 


Odometry  is  needed  to  localize  the  vehicle.  This  function  requires  encoder  and 
gyroscope  readings.  The  vehicle  position  is  evaluated  every  encoder  reading.  At  each 
iteration,  the  distance  d  traveled  is  calculated. 


d  =  V(t)(n)(D) 


where 

d  =  distance  [m] 

V  =  current  speed  [ips] 
t  =  odometry  sampling  time  [s] 

D  =  wheel  diameter  [m]. 

The  position  increments,  Ax  and  Ay,  arc  evaluated  using  the  vehicle  yaw  angle  a.  The 
angle  is  gyro-stabilized  as  explained  in  the  Microstrain  documentation  [8,  p.7].  Also, 
when  the  autonomous  mode  is  initialized,  the  angle  measured  becomes  the  offset  so 
everytime  this  mode  is  selected  with  the  remote  control,  the  angle  is  set  to  zero. 


Ax  =  d  sin  a 


Ay  =  d  cos  a 


Then,  these  increments  arc  added  to  the  previous  position  to  get  a  global  vehicle 
position. 

x[n]  =  x[n  —  1]  +  Ax[n] 
y[n]  =  y[n  -  1]  +  A y[n] 


The  accuracy  of  this  odometry  calculation  is  sufficient  for  a  short  test  distance,  but  the 
error  increased  at  every  iteration.  The  odometry  could  be  improved  by  combining 
gyroscope  data  with  Global  Positionning  System  data.  With  the  MPC555,  this 
combination  is  not  possible  since  only  two  serial  ports  are  available:  one  for 
multiprocessor  communication  and  one  for  the  gyroscope. 
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4.10  A-to-B  Mobility 


The  purpose  of  this  function  is  to  move  the  robot  from  one  point  to  another.  Since  the 
odometry  isn’t  very  accurate,  the  result  of  the  mobility  is  also  not  accurate.  However,  it 
is  reasonable  over  a  short  distance. 

Position  errors  in  x  and  y  axis  arc  determined  by  the  next  formulas. 


ax=xb-x 

AY  =  Yb-Y 


Where 

(. XBiYb )  =  arrival  position  coordinates 
(X,Y)  =  current  position  coordinates 
(AX,  AT)  =  position  errors 

The  mission  is  successful  if  the  position  respects  a  tolerance  of  20  cm  on  X  and  Y. 

To  move  the  robot,  the  idea  is  to  determine  where  is  the  arrival  point  and  which  side  to 
rotate.  First,  the  final  point  is  positioned  in  a  quadrant  around  the  robot.  The  quadrant 
determination  is  presented  in  Table  2.  Figure  6  sketches  the  geometry  angles  of  the 
different  configurations  to  facilitate  the  comprehension  of  the  next  explanations. 


AX 

AY 

Quadrant 

>=0 

>=  0 

1 

<0 

>=0 

2 

<0 

<0 

3 

>=0 

<0 

4 

Table  2:  Quadrant  determination 

Then,  the  absolute  heading  a  from  the  current  position  to  the  target  is  evaluated  with 
the  next  formula.  One  exception  is  for  AT=  0,  a  =  90  deg. 


a  =  arctan 


abs 


The  angle  a  is  then  expressed  in  the  same  reference  system  as  the  gyroscope.  The 
result  is  [3.  It  means  that  the  zero  degree  angle  of  the  gyroscope  is  equivalent  to  [3  =  0. 
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Departure 


X 


B) 


Departure 


X 


X 


Figure  6:  Representation  of  the  different  configurations  of  the  arrival  point  in  accordance  with  the  current  position. 
A)  Quadrant  1.  B)  Quadrant  2,  C)  Quadrant  3  and  D)  Quadrant  4. 


Quadrant 

Formula  to  get  [3 

1 

P  =  a 

2 

p=360-a 

3 

P  =  180  +  a 

4 

p  =  180 -a 

Table  3:  Equations  to  evaluate  (3  in  accordance  with  the  quadrants 
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Each  quadrant  required  a  different  formula  to  execute  this  transformation.  Table  3 
presents  the  equations  to  get  [3. 

The  useful  angle  to  orient  the  robot  is  the  angle  y  between  the  current  robot  orientation 
9  and  the  target  angle  [3.  y  represents  the  angle  the  robot  has  to  rotate.  As  the  algorithm 
works  with  positive  angles  between  0  and  360  degrees,  for  negative  angles  y  =  y+  360. 
Same  rule  for  9. 


y=P-e 


Now  that  the  rotational  movement  requirement  has  been  evaluated,  a  PID  servo-control 
function  is  used  to  determine  the  robot  movement.  For  each  y  45°  slice,  a  different 
movement  is  established.  It  considers  that  for  some  angles,  it  is  more  practical  to  go 
backward  than  forward  and  sometimes  it  is  better  to  go  backward  a  little  to  reorient  the 
robot  and  then  forward,  like  driving  a  car.  For  instance,  if  yis  60  degrees,  it  is 
impossible  for  the  vehicle  to  turn  on  a  short  radius,  so  it  necessitates  a  big  space  to 
describe  an  arc  to  orient  itself  in  this  direction.  It  is  more  appropriate  to  go  backward 
left  up  to  a  smaller  y.  When  y  get  between  0-45  degrees,  then  the  robot  goes  forward 
right.  Thus,  the  space  required  to  orient  the  robot  is  smaller.  Table  4  shows  the  XTB 
behavior  in  each  y45°  degree  slice. 

The  Kp  constant  is  a  proportional  constant  (Kp  =  195).  It  has  been  determined  to  get  a 
maximum  PWM  value  for  a  5  degree  y,  where  a  maximum  steering  angle  is  required  to 
turn  as  quickly  as  possible.  The  values  3710  and  4179  arc  the  zero  degree  steering 
angle  PWM  values  of  the  steering  servos.  V  is  a  speed  requested.  The  value  of  V  has 
been  established  to  13  rpm  in  the  experiment.  Though  this  is  an  arbitrary  speed  offering 
good  progression,  it  can  be  accelerated  or  decelerated. 

Then,  the  steering  PWM  values  arc  compared  to  the  servos  limits.  If  they  arc  over  the 
maximum  or  below  the  minimum,  the  values  arc  changed  for  the  limit  values.  The 
steering  and  the  motor  can  now  be  commanded. 

The  mobility  function  is  executed  continuously  every  gyroscope  reading.  When  the 
wander  mode  program  is  chosen,  there  is  no  A-to-B  mobility  available.  The  opposite  is 
true  too.  They  do  not  have  to  be  integrated  together  since  they  shouldn’t  be  used  in  the 
same  applications. 

5.  Program  algorithms 


This  section  presents  simplified  diagrams  of  the  algorithms  developed  to  control  the 
XTB.  They  give  a  good  idea  of  the  software  algorithm  content  and  the  relations 
between  the  different  tasks.  A  short  explanation  of  the  RTEMS  specific  terms  used  is 
previously  given. 
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Behavior 


Correction 


Y [deg] 

0  and  360 
0<y  <45 
45<y  <90 
90<y < 1 35 
135<y  <180 
180<y<225 
225<Y<270 
270<y<315 
3 1 5<y<360 


Straight  forward 
Right  forward 
Left  backward 
Left  forward 
Right  backward 
Left  backward 
Right  forward 
Right  backward 
Left  forward 


0 

Kpx  Y 
KPx  y 
Kpx  y 
KPx  y 

KP  x  (y—  360) 
KP  x  (y-360) 
KP  x  (y-360) 
KP  x  (y-360) 


Front  steering  PWM 

Back  steering  PWM 

Speed 

3710 

4179 

V 

3710-correction 

4179+correction 

V 

3710+correction 

4179-correction 

-V 

3710+correction 

4179-correction 

V 

3710-correction 

4179+correction 

-V 

3710-correction 

4179+correction 

-V 

3710+correction 

4179-correction 

V 

3710+correction 

4179-correction 

-V 

3710-correction 

4179+correction 

V 

5.1  Terminology 


First  of  all,  some  real-time  system  expressions  has  to  be  explained. 

Task:  ’’The  smallest  thread  of  execution  which  can  compete  on  its  own  for  system 
resources.”  [l,p.30] 

Semaphore:  ’’Protected  variable  whose  value  can  be  modified  only  when  creating, 
obtaining  or  releasing  directives.  RTEMS  supports  both  binary  and  counting 
semaphore.”  [1,  p.89] 

Message-queue:  ’’Variable  length  buffer  where  information  can  be  store  to  support 
communication.”  [l,p.  103] 

Event:  ”An  event  flag  is  used  by  a  task  to  inform  another  task  of  the  occurence  of  a 
significant  situation.”  [1 ,  p.  1 19] 


5.2  Simplified  algorithm  diagrams 

Due  to  the  complexity  of  the  program  software  algorithms,  simplified  diagrams  are 
presented  in  this  section.  The  first  one  is  the  initialization  routine.  It  defines,  initializes, 
starts  and  deletes  tasks,  semaphores  and  message  queues.  Figure  7  presents  the  main 
function  of  the  real-time  system  that  setups  all  the  program. 

The  second  diagram  is  the  TPU  setup  in  Figure  8.  It  configures  the  time  processor  unit. 
No  task  using  the  TPU  can  run  without  having  received  an  event  from  the  setup  as 
being  completed.  Thus,  it  allows  the  tasks  to  proceed. 

Follow  the  tpu  reset  task  in  Figure  9.  It  resets  the  TPU  channels  that  arc  not  set 
properly. 

The  remote  control  task  (Figure  10)  reads  the  signals  from  the  receiver  to  get  their  duty 
cycles.  It  determines  the  mode  (manual  or  autonomous),  the  remote  speed  and  the 
steering  commands.  In  manual  mode,  the  task  commands  the  motor  and  the  steering 
servos.  It  also  executes  a  simple  reactive  avoidance  by  forbidding  any  movement  in  a 
detected  obstacle  direction. 

Figure  1 1  shows  the  infrared  range  detection  diagram.  The  sensors  detect  the  presence 
of  obstacles  and  send  warnings  to  other  tasks.  It  allows  the  program  to  realize  simple 
reactive  avoidance. 

To  know  the  vehicle  speed,  an  encoder  reading  task  is  developed,  see  Figure  12.  It 
counts  the  encoder  pulses  and  calculates  the  speed.  In  autonomous  mode,  it  then 
compares  it  to  the  requested  speed,  evaluates  the  correction  required  to  the  motor 
command  and  commands  the  engine. 
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Figure  7:  Program  initialization  diagram 


The  next  task.  Figure  13,  is  a  function  to  wander  in  an  indoor  environment  with 
obstacles.  It  is  realized  only  in  autonomous  mode.  When  an  object  is  detected,  the 
robot  stops  running  in  the  obstacle  direction.  It  reverses  while  turning  for  6  seconds, 
then  stops  2  seconds  and  continues  straight  forward. 

To  control  the  platform  tilt,  two  different  control  processes  were  tried.  The  first  one, 
Figure  14,  uses  feedback  from  a  dual-axis  tilt  sensor  and  four  suspension  position 
sensors  to  control  the  platform  orientation.  It  involves  a  proportional  correction  and 
tries  to  keep  centered  the  pistons.  The  second  one,  Figure  15,  uses  only  the  tilt  sensor 
with  a  PID  loop.  The  first  control  is  prefered  for  the  application  because  it  respects  the 
piston  limits.  Also,  it  keeps  centered  the  piston  as  much  as  possible  to  optimize  the 
suspension  movement. 

The  last  diagram  (Figure  16)  demonstrated  a  rudimentary  A-to-B  mobility  algorithm. 
For  a  given  arrival  point,  it  orients  the  vehicle  in  this  point  direction  and  then  reaches  it. 
The  robot  analyzes  the  rotation  required  and  executes  backward  or  forward  movements 
and  right  or  left  steering  to  optimize  the  space  required  to  orient  itself.  A  20  cm 
tolerance  is  applied  in  x  and  y  axes  to  complete  a  successful  mission.  Finally,  to  avoid 
collision,  if  an  obstacle  is  detected,  no  movement  is  allowed  in  this  direction.  When  the 
obstacle  is  removed,  the  robot  continues.  In  future  work,  an  obstacle  avoidance  routine 
will  be  integrated  to  this  algorithm. 
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Figure  8:  TPU  setup  diagram 
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Figure  9:  TPU  reset  diagram 
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Figure  10:  Remote  control  diagram 
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Figure  11:  IR  range  detection  diagram 
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Figure  12:  Diagram  for  the  encoder  reading  and  the  autonomous  mode  speed  control 
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Wander.c 


Obtain  semaphore  6 


Receive  obstacle  location  message 


Receive  autonomous  mode  event 


Receive  current  rpm  message 


Obstacle  forward  and  go  forward 


True  rpm  reference  =13  (backward) 
Steering  turns  max  right 


Obstacle  backward  and  go  backward 


No  obstruction  to  current  movement,  continue. 


JTrue  rpm  reference  =  -13  (forward) 
Steering  turns  max  right 


Send  rpm  reference  message 


Task  wakes  after  6  sec 


Stop,  rpm  reference  =  0 


Send  rpm  reference  message 


Task  wakes  after  2  sec 


Continue  to  advance  in  the  same  direction  than 
before  meeting  the  obstacle.  Steering  straight. 


Send  rpm  reference  message 


Figure  13:  Wander  task  diagram 
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Figure  14:  Platform  balance  diagram 
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Figure  15:  Platform  PID  loop  diagram 
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Figure  16:  A-to-B  mobility  diagram 
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6.  Conclusion 


The  document  has  described  the  XTB:  the  mechanical  components,  the  sensors  and  the 
microprocessor.  Being  small,  the  R/C  truck  can  easily  be  manipulated  by  a  human. 
Some  other  advantages  arc  that  it  is  inexpensive  and  research  using  this  vehicle  can, 
later,  be  transfered  to  bigger  and  more  expensive  platforms. 

The  XTB  is  useful  for  testing  technologies,  concepts  and  algorithms,  and 
familiarization  with  the  powerful  MPC555  microprocessor  and  the  real-time  operating 
system  RTEMS.  The  program  algorithms  presented  in  this  memorandum  are  mainly  an 
active  platform  to  keep  it  horizontal  when  the  vehicle  tilts,  a  teleoperation  mode  to 
direct  manually  the  robot,  a  wander  mode  to  run  randomly  avoiding  the  obstacles,  and  a 
rudimentary  A-to-B  mobility  to  go  from  point  A  to  point  B  autonomously.  These  tasks 
involve  other  parts  as  the  IR  sensors  to  detect  obstacles,  the  encoder  to  get  the  speed, 
the  tilt  sensors  to  know  the  platform  tilt  and  the  gyroscope  to  obtain  odometry  data. 

With  the  first  control  level  complete,  a  second  control  level  may  be  added  that  provide 
navigation  and  arbitration.  The  installation  of  a  camera  would  be  useful  for  the 
navigation  control  level. 
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PWM  value 


Annex  A 

Calibration  of  the  servos 


Each  servo  has  been  calibrated  to  get  a  relation  between  the  pulse  width  and  the 
resulting  angle  for  the  steering  and  the  IR  sensor  bumpers,  or  the  resulting  speed  for  the 
suspension  pistons.  The  simple  calibration  method  employed  has  been  to  send  a 
specific  PWM  value  and  to  measure  the  resulting  angle  or  speed. 


A.1  IR  sensors 

To  evaluate  the  infrared  sensor  angle,  a  protractor  has  been  fixed  to  the  bumper  end  so 
the  zero  degree  indicates  the  horizontal  orientation  of  the  sensor.  At  this  position,  the 
front  sensor  detects  straight  in  front  of  the  vehicle.  A  positive  angle  corresponds  to  an 
upper  detection  than  the  horizontal.  Figure  A.2  for  the  real-  bumper  and  Figure  A.  1  for 
the  front  one,  present  the  regression  line  obtained  for  each  graphic  with  its  equation.  A 
correlation  factor  (R)  indicates  the  goodness  of  the  fitting  curve.  The  fit  is  very  good  in 
both  cases  with  R  =  0.9982  for  the  real-  sensors  and  R  =  0.9944  for  the  front  ones. 


Scan  angle  (degree) 


Figure  A.  1:  Front  IR  scan  servo  calibration  curve 
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Servo  angle  (Degree) 


Figure  A.2:  Back  IR  scan  servo  calibration  curve 


A.2  Steering 

For  the  steering  servos  calibration,  the  calibration  is  more  complex  than  for  the  bumper 
angles.  When  the  steering  moves,  the  center  of  the  tire  tread  doesn’t  stay  at  the  same 
place.  It  describes  a  circular  motion  around  the  bracket  pivot.  Thus,  it  is  impossible  to 
install  the  protractor  above  the  tire  and  measure  the  variation  in  orientation  for  different 
PWM  commands.  The  alternative  technique  is  to  mark  a  line  parallel  to  the  tire  wall  for 
each  PWM  command.  Then,  each  line  is  compared  angularly  to  the  line  obtained  at 
zero  angle.  Thus,  it  is  possible  to  get  a  relation  between  the  pulse  width  and  the 
steering  angle.  Figure  A.3  and  A.4  present  this  relation  for  the  front  and  the  back 
steering  respectively.  The  calibration  method  is  not  very  accurate  and  the  position  of 
the  tire  is  not  exactly  the  same  for  a  same  PWM  command.  The  steering  rod  clearance 
induces  hysteresys  with  the  floor  friction  and  inaccuracy.  This  increases  the  error  in 
odometry  data.  Nonetheless,  the  fitting  curve  for  the  front  steering  calibration  has  a 
correlation  factor  of  0.9964  and  the  back  steering  calibration  of  0.9967.  Even  if  the 
lineal-  regression  fit  well,  the  mechanism  and  the  measurement  method  are  not  accurate. 
Consequently,  another  way  to  determine  the  steering  angle  is  required.  It  is  why  a 
lineal'  sensor  should  be  installed  on  the  steering  rod.  It  would  give  the  exact  angle  of 
the  steering.  By  looping  a  correction  function,  it  would  be  possible  to  modify  the  PWM 
value  and  to  get  the  desired  steering  angle. 
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speed 


Figure  A.5:  Back-left  piston  relation  between  speed  and  PWM 


A.3  Suspension  piston 

The  suspension  calibration  is  easier  than  for  the  steering,  but  not  very  accurate.  A 
software  algorithm  samples  the  time,  the  piston  position  sensor  output  and  the  servo 
PWM  value.  The  pulse  width  stays  constant  for  several  samplings  before  to  be 
changed.  After  many  trials,  a  graphic  of  the  relation  between  the  piston  speed  and  the 
servo  pulse  width  is  built  for  each  suspension.  Figures  A.6,  A.5,  A.7  presents  the 
results.  A  linear  fitting  line  gives  the  approximative  equation  of  the  system  reaction. 
Even  if  the  system  doesn’t  seem  to  be  linear-,  this  approximation  is  good  enough  for  the 
present  application.  The  front-left  piston  graph  has  not  been  done  since  it  was  possible 
to  conclude  with  the  others  that  a  linear  estimation  would  be  appropriate  for  the  robot 
activity.  The  fittness  curve  data  are  not  utilized  in  the  platform  balance  algorithm  since 
they  are  not  accurate  and  change  too  much  from  one  trial  to  another.  The  linear 
constants  used  in  the  program  have  been  set  by  trials  and  errors  to  get  a  good  reaction 
of  the  system. 
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Figure  A. 6:  Back-right  piston  relation  between  speed  and  PWM 
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Figure  A.  7:  Front-right  piston  relation  between  speed  and  PWM 
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Annex  B 

List  of  Acronyms 


The  list  shown  in  Table  B.l  defined  every  acronyms  used  in  this  document. 


Acronym 

Definition 

ADC 

Analog-to-Digital  Converter 

BL 

Back  left 

BR 

Back  right 

CPU 

Central  Processor  Unit 

FQM 

Frequency  Measurement  Function 

FL 

Front  left 

FR 

Front  right 

IPC 

Inter  Process  Communication 

IR 

Infrared  sensors 

KD 

Derivative  constant 

KI 

Integrative  constant 

KP 

Proportional  constant 

NITC 

New  Input  Transition  /  Input  Capture  Function 

PWM 

Pulse  Width  Modulation 

RTEMS 

Real  Time  Executive  for  Multiprocessor  Systems 

SCI 

Serial  Communication  Interface 

TCR 

Time  Count  Register 

TPU 

Time  Processor  Unit 

XTB 

Extreme  Test  Bed 

Table  B.  1:  Acronym  list 
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